/*
 * @Descripttion: 
 * @version: 
 * @Author: Glorill
 * @Date: 2021-10-28 15:21:44
 * @LastEditors: Glorill
 * @LastEditTime: 2022-03-05 21:44:13
 */
#ifndef PAN_TILT_H
#define PAN_TILT_H

#include "SRML.h"
#include "math.h"
#include "Serial_Plot.h"
#include "pan_tilt_AHRS.h"

extern Motor_GM6020 YawMotor;
extern Motor_GM6020 PitchMotor;

const float PITCH_MAX = 394;
const float PITCH_MIN = 341;
// const float PITCH_LIGHT = 260; //灯条机械限位

//水平时的云台角
const float Pitch_Initial = 390;

//朝前方时的云台角
extern float Yaw_Initial;

const float PITCH_BLANCE_VOLTAGE = 0;

class C_Pan_Tilt
{
	private:
        void Pithch_Angle_Limit(void);
        void Send_Motor_Message(void);

    public:
        enum E_PanTiltState
        {
            DISABLE = 0,    //输出0
            NORMAL = 1,     //正常输出
            BLANCE = 2,     //pitch输出平衡力矩，yaw输出0
        } state;

        void Init(void);
        void Control(void);

        void SetPitch(float pitch_angle);
        void SetYaw(float yaw_angle);
        void SetState(E_PanTiltState _state) { state = _state; };

        float GetNowPitch(void);
        float GetNowYaw(void);
        float GetTargetPitch(void);
        float GetTargetYaw(void);
};

extern Serial_Plot PitchData;
extern C_Pan_Tilt Pan_Tilt;
extern C_PAN_TILT_AHRS pan_tilt_AHRS;

#endif
